!
! absfit - fitting absolute deviations
!
!
! Copyright © 2011 F.Hroch (hroch@physics.muni.cz)
!
! This file is part of Munipack.
!
! Munipack is free software: you can redistribute it and/or modify
! it under the terms of the GNU General Public License as published by
! the Free Software Foundation, either version 3 of the License, or
! (at your option) any later version.
! 
! Munipack is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
! GNU General Public License for more details.
! 
! You should have received a copy of the GNU General Public License
! along with Munipack.  If not, see <http://www.gnu.org/licenses/>.
!

module robfit
  
  implicit none

  logical, parameter, private :: debug = .true.
  integer, parameter, private :: dbl = selected_real_kind(15)
  real(dbl), parameter, private :: rad = 57.295779513082322865_dbl
  real(dbl), dimension(:), allocatable, private :: x,y,u,v
  real(dbl), private :: mad
  integer, private :: n

  private :: minfun

contains

  subroutine minfun(np,p,fvec,iflag)
    
    use rfun

    integer, intent(in) :: np
    integer, intent(inout) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(np), intent(out) :: fvec
    real(dbl) :: s,c,sf,sc,sx,sy,rx,ry,rpx,rpy,dx,dy
    integer :: i

    if( iflag == 0 .and. debug ) write(*,*) '#t:',p

    c = cos(p(4))
    s = sin(p(4))

    sf = 0.0_dbl
    sc = 0.0_dbl
    sx = 0.0_dbl
    sy = 0.0_dbl
    do i = 1, n
       dx = c*x(i) - s*y(i)
       dy = s*x(i) + c*y(i)
       rx = u(i) - (p(1) + p(3)*dx)
       ry = v(i) - (p(2) + p(3)*dy)
       rpx = tukey(rx/mad)
       rpy = tukey(ry/mad)
       sx = sx + rpx
       sy = sy + rpy
       sc = sc +  rpx*dx + rpy*dy
       sf = sf + (rpx*dy - rpy*dx)*p(3)
!       write(*,'(8f7.3)') u(i),v(i),x(i),y(i),sx,sy,sc,sf
    end do
    fvec = (/-sx,-sy,-sc, sf/)*mad
!    write(*,*) "fvec=",fvec

  end subroutine minfun
  

  subroutine robmin(type,a,d,xc,yc,w,h,xx,yy,xmad,acen,dacen,dcen,ddcen,sc,dsc,pa,dpa,rms,s0)

    ! find proper transformation by robust mean squares

    use rfun
    use astrotrafo

    ! parameters
    character(len=*),intent(in) :: type
    real(dbl),dimension(:),intent(in) :: a,d,xx,yy
    real(dbl),intent(in) :: xmad,xc,yc,w,h
    real(dbl),intent(inout) :: acen,dcen,sc,pa
    real(dbl),intent(out) :: dacen,ddcen,dsc,dpa,rms,s0

    ! fitting
    integer, parameter :: npar = 4
    integer, parameter :: lwa = (npar*(3*npar+13))/2
    integer, parameter :: lr = (npar*(npar+1))/2
    real(dbl),dimension(npar) :: p,dp
    real(dbl),dimension(npar) :: fvec
    real(dbl),dimension(npar) :: diag,wa1,wa2,wa3,wa4,qtf
    real(dbl),dimension(npar,npar) :: fjac,cov
    real(dbl),dimension(lr) :: r
    integer,dimension(npar) :: ipvt
    integer :: iter,i,j,m,info, nfev
    real(dbl) :: sum1,sum2,wx,wy
    real(dbl) :: rx,ry,s,c,xtol,sc0
    real(dbl), dimension(1) :: a0,d0
    type(TypeProjection) :: t

    if( size(a) < 5 ) stop 'Robust fitting needs five stars at least.'


    xtol = epsilon(xtol)
    mad = xmad

    ! normalization
    n = size(a)
    allocate(x(n),y(n),u(n),v(n))
    sc0 = (w + h)/2.0_dbl
    x = (xx - xc)/sc0
    y = (yy - yc)/sc0

    ! initial projection
    t%type = type
    t%acen = acen
    t%dcen = dcen
    t%xcen = 0.0_dbl
    t%ycen = 0.0_dbl
    t%scale = 1.0_dbl
    t%pa = 0.0_dbl
    call trafo(t,a,d,u,v)

    ! initial estimation of pa
    p = (/0.0_dbl, 0.0_dbl, 1.0_dbl, 0.0_dbl/)
    p(3) = sc0/sc
    p(4) = pa/rad

    do iter = 1, 10

       if( debug ) write(*,*) '# iter:',iter

       call hybrd(minfun,npar,p,fvec,xtol,200*(npar+1),npar-1,npar-1, &
            0.0_dbl, diag,1,100.0_dbl,1,info,nfev,fjac,npar, &
            r,lr,qtf,wa1,wa2,wa3,wa4)

       if( debug ) write(*,*) '# par=',p
       if( debug ) write(*,*) '# info=',info
       if( debug ) write(*,*) '# fjac=',(fjac(i,:),i=1,size(fjac,1))
       if( debug ) write(*,*) '# r=',r
       if( debug ) write(*,*) '# qtf=',qtf
       if( debug ) write(*,*) '# fvec=',fvec

       if( info == 0 ) stop 'Improper input parameters.'

       ! center of FOV correction
       call invtrafo(t,(/p(1)/),(/p(2)/),a0,d0)

       acen = a0(1)
       dcen = d0(1)
       sc = sc0/p(3)
       pa = rad*p(4)

       if( debug ) write(*,*) '# a,d,ac,dc=',acen,dcen,sc*(acen-t%acen),sc*(dcen-t%dcen)

       ! residual sum
       s0 = 0.0_dbl
       sum1 = 0.0_dbl
       sum2 = 0.0_dbl
       c = cos(p(4))
       s = sin(p(4))

       do i = 1, n
          rx = u(i) - (p(1) + p(3)*(c*x(i) - s*y(i)))
          ry = v(i) - (p(2) + p(3)*(s*x(i) + c*y(i)))
          s0 = s0 + rx**2 + ry**2
          wx = rx/mad
          wy = ry/mad
          sum1 = sum1 + tukey(wx)**2 + tukey(wy)**2
          sum2 = sum2 + dtukey(wx) + dtukey(wy)
!          write(*,*) rx,ry,wx,wy,tukey(wx),tukey(wy),dtukey(wx),dtukey(wy)
       enddo

       ! covariance matrix
       cov = 0.0_dbl
       m = 1
       do j = 1,npar
          do i = j,npar
             cov(i,j) = r(m)
             cov(j,i) = r(m)
             m = m + 1
          end do
       end do
    
       ipvt = (/ (i,i=1,npar) /)
       call covar(npar,cov,npar,ipvt,epsilon(cov),wa1)
       if( debug ) write(*,*) '# inverse:'
       if( debug ) write(*,*) '# ',cov
       
       s0 = mad**2*sum1/sum2**2*n**2
       do i = 1,npar
          dp(i) = sqrt(s0*cov(i,i)/(n - npar))
       enddo

       write(*,'(a,en12.3,en13.3,f8.1,2f14.5)') "=RFIT> ",s0,sc,pa,acen,dcen

       ! finish when parameters are appropriate small
       if( iter > 1 .and. abs(p(1)) < 0.1*dp(1) .and. abs(p(2)) < 0.1*dp(2) ) exit !.and. &
!            abs(rp) < dpa .and. abs(rc) < dsc ) exit

!       if( sqrt(p(1)**2*cos(t%dcen/rad)**2 + p(2)**2) < xtol ) exit
!       if( sqrt(rx**2*cos(t%dcen/rad)**2 + ry**2) < tol ) exit
       ! it's probably important to use a non-Euclidean metric
       ! we are ignoring angle and scale changes 

       ! new projection by using of fitted parameters
       t%acen = acen
       t%dcen = dcen
       call trafo(t,a,d,u,v)

    end do

    ! deviations of output parameters
    dacen = dp(1)
    ddcen = dp(2)
    dsc = sc*(dp(3)/p(3))
    dpa = rad*dp(4)
    rms = sqrt(s0/(n - npar))

    if( debug ) write(*,*) '# solution:',p
    if( debug ) write(*,*) '# uncerts.: ',dp
    if( debug ) write(*,*) '# diag:  ',diag
    if( debug ) write(*,*) '# scale: ',sc,dsc
    if( debug ) write(*,*) '# s0,sum1,sum2,rms: ',s0,sum1,sum2,rms

    deallocate(u,v,x,y)

  end subroutine robmin

end module robfit

